#include "math.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

#include "hardware.h"
#include "led_driver.h"
#include "pxx_driver.h"
#include "cppm_driver.h"
#include "dac_audio_driver.h"
#include "trim_driver.h"
#include "switch_driver.h"

#include "power_driver.h"
#include "uart_driver.h"
#include "ee_driver.h"
#include "lcd_driver.h"
#include "string.h"

#include "dialog_main.h"
#include "signal.h"
#include "stdio.h"
#include "dialog_cali.h"

#include "main.h"
#include "task_audio.h"
#include "task_UI.h"
#include "task_system.h"
#include "task_mix.h"

#if 0
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif

PUTCHAR_PROTOTYPE
{
    char t_char;
    t_char =ch;
    uart_driver.write(&uart_driver, &t_char, 1, 0);
    return ch;
}
#endif


#define PRIO_0  (0) 
#define PRIO_1  (1)
#define PRIO_2  (2)
#define PRIO_3  (3)
#define PRIO_4  (4)//max priority

#define TASK_A_STACK_SIZE   (128)
#define TASK_B_STACK_SIZE   (128)




#if 1
short wave_form[1024]=
{
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010,
 2048,2085,2123,2160,2197,2233,2268,2303,2336,2369,
  2400,2430,2458,2485,2510,2533,2554,2573,2590,2605,
  2618,2629,2637,2643,2646,2648,2646,2643,2637,2629,
  2618,2605,2590,2573,2554,2533,2510,2485,2458,2430,
  2400,2369,2336,2303,2268,2233,2197,2160,2123,2085,
  2048,2010,1972,1935,1898,1862,1826,1792,1758,1726,
  1695,1665,1637,1610,1585,1562,1541,1522,1505,1490,
  1477,1466,1458,1452,1448,1448,1448,1452,1458,1466,
  1477,1490,1505,1522,1541,1562,1585,1610,1637,1665,
  1695,1726,1758,1792,1826,1862,1898,1935,1972,2010
};
short wave_formB[1024] =
{
    2000,2100,2200,2300,2400,2500,2600,2700,2800,2900,
    2900,2800,2700,2600,2500,2400,2300,2200,2100,2000,
    1900,1800,1700,1600,1500,1400,1300,1200,1100,1000,
    1100,1200,1300,1400,1500,1600,1700,1800,1900,2000,
};
#endif

void taskA(void* pdata)
{
    led_driver.open(&led_driver);
    
    //pxx driver testing
    if(0)
    {
        char pxx_frame[18]={0,0,0,0,0,0};
        pxx_driver.open(&pxx_driver);
        pxx_driver.write(&pxx_driver,pxx_frame,sizeof(pxx_frame), 0);
        
        vTaskDelay(10000);//20S
        pxx_driver.close(&pxx_driver);
    }
    
    //cppm driver testing
    if(0)
    {
        uint16_t cppm_frame[6]={15*100,15*110,15*120,15*130,15*140,15*150};
        cppm_driver.open(&cppm_driver);
        cppm_driver.write(&cppm_driver, cppm_frame, sizeof(cppm_frame), 0);

        vTaskDelay(10000);//20S
        cppm_driver.close(&cppm_driver);
    }
    
    //power driver testing
    if(0)
    {
        char t;
        power_driver.open(&power_driver);
        power_driver.read(&power_driver,&t,sizeof(t), 0);
        power_driver.write(&power_driver, &t, sizeof(t), 0);
        
        vTaskDelay(10000);//20S
        power_driver.close(&power_driver);
        
    }    //dac audio driver testing

#if 0
    if(0)
    {
        dac_audio_driver.open(&dac_audio_driver);
        while(1)
        {
            static char t;
            static char a;
            
            //t=1;
            //led_driver.write(&led_driver,&t,sizeof(t),0);
            dac_audio_driver.write(&dac_audio_driver, wave_form, sizeof(wave_form), 0);
            //t=0;
            //led_driver.write(&led_driver,&t,sizeof(t),0);
            dac_audio_driver.write(&dac_audio_driver, wave_formB, sizeof(wave_formB), 0);
            
            //vTaskDelay(50);
            if(1){
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_FREQ, 100000);
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_VOLUME, 0x3f &0x7f);
            }
            
            if(0){
                if( (a++ &0x1) ==0)
                {
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_FREQ, 100000);
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_VOLUME, a &0x7f);
                }else
                {
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_FREQ, 50000);
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_READ_VOLUME, (uint32_t)&t);
                }
            }
        }
        
        //dac_audio_driver.close(&dac_audio_driver);
    }
#endif

    if(0)
    {
        ee_driver.open(&ee_driver);
        char buffer[123];
        int counter=0; 
        
        for(int i=0; i<20; i++)
        {
            //memset(buffer,0x55,sizeof(buffer));
            //ee_driver.write(&ee_driver, buffer, sizeof(buffer), (OFFSET)counter);
            memset(buffer, 0xaa, sizeof(buffer));
            ee_driver.read(&ee_driver, buffer, sizeof(buffer), (OFFSET)counter);
            counter +=sizeof(buffer);
        }
        
    }

#if (0)
    //test kalman filter
    if(1)
    {
        dac_audio_driver.open(&dac_audio_driver);
        dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_VOLUME, 64);
        
        for(int i=0; i<sizeof(wave_form)/sizeof(wave_form[0]); i++)
        {
#define PI (3.14159)
            static short x;
            float y;
            y = (sin( (2*PI)*i/128.0) )*1600.0;
            y += (random16()%400);
            wave_form[i] = (short)y;
            wave_formB[i] =(short)y;
            x++;
        }
        
#include "kalman1D.h"
#include "kalman2D.h"
        float Q[4]={200.0, 200.0, 2.0, 2.0};
        float R[4]={200.0, 200.0, 20.0, 20.0};
        float x[2];
        x[0] =wave_form[0];
        x[1] =wave_formB[0];
        float P[1] ={100.0};

        //initKalman1D( Q, R, x,P);
        initKalman2D(Q, R, x, P);

        for(int i=0; i<sizeof(wave_form)/sizeof(wave_form[0]); i++)
        {
            x[0] =wave_form[i];
            x[1] =wave_formB[i];
            //kalman1Dupdate(x, R, 0.4);

            kalman2Dupdate(&x[0], &x[1], &R[0], &R[1], 0.4);
            //wave_form[i] =(int)(x[1] +2048.0);
            wave_form[i] =(int)(x[0] +2048.0);
        }
        while(1)
        {
            static char t;
            static char a;
            
            dac_audio_driver.write(&dac_audio_driver, wave_form, sizeof(wave_form), 0);
            //t=0;
            //led_driver.write(&led_driver,&t,sizeof(t),0);
            //dac_audio_driver.write(&dac_audio_driver, wave_formB, sizeof(wave_formB), 0);
            
            /*
            CoTickDelay(50);
            {
                if( (a++ &0x1) ==0)
                {
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_FREQ, 100000);
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_VOLUME, a &0x7f);
                }else
                {
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_WRITE_FREQ, 50000);
                    dac_audio_driver.ioControl(&dac_audio_driver, IO_CONTROL_DAC_AUDIO_READ_VOLUME, (uint32_t)&t);
                }
            }
            */
        }
        
    }
#endif
    
    //test lcd_driver
    if(0)
    {
        lcd_driver.open(&lcd_driver);
    
        lcd_driver.ioControl(&lcd_driver, IO_CONTROL_LCD_SET_XY, 0);
    
        while(1)
        {
            char test;
            char lcd_buffer[108]={0xFE,0xDC, 0xBA, 0x98, 0x76, 0x54, 0x32, 0x10, 0xFF, 0xFF};
        
            lcd_driver.ioControl(&lcd_driver, IO_CONTROL_LCD_SET_XY, 0x500);
            for (test=0; test<64; test++)
            {
                lcd_driver.write(&lcd_driver, &lcd_buffer, sizeof(lcd_buffer), 0);
            }
        }
    }

    //main process
    while(1)
    {
        char t;
        
        t=1;
        //led_driver.write(&led_driver,&t,sizeof(t),0);
    }
}

void taskB(void* pdata)
{
    while(0)
    {
        ee_driver.open(&ee_driver);
        char buffer[123];
        int counter=0; 
        
        for(int i=0; i<10; i++)
        {
            memset(buffer,0x55,sizeof(buffer));
            //ee_driver.write(&ee_driver, buffer, sizeof(buffer), (OFFSET)counter);
            memset(buffer, 0xaa, sizeof(buffer));
            ee_driver.read(&ee_driver, buffer, sizeof(buffer), (OFFSET)counter);
            counter +=sizeof(buffer);
        }
        
    }
    while(1)
    {
        char t;
        t=0;
        //led_driver.write(&led_driver,&t,sizeof(t),0);
    }
}

/// global dialogs
/// these dialogs could accept global messages

#include "task_audio.h"

int main()
{
    hardwareInit();

    switch_driver.open(&switch_driver);
    trim_driver.open(&trim_driver);
    
    uart_driver.open(&uart_driver);
   
    queueAudio =xQueueCreate(4, sizeof(AudioMail));

    /// read TransmiterFile;
    /// read modelFile;

    /// create tasks
    //xTaskCreate(taskA, (const signed char *)"taskA", TASK_A_STACK_SIZE, NULL, 2, NULL);
    //xTaskCreate(taskB, (const signed char *)"taskB", TASK_B_STACK_SIZE, NULL, 2, NULL);
    xTaskCreate(taskUI, (const signed char *)"taskUI", TASK_UI_STACK_SIZE, NULL,2, NULL);
    //xTaskCreate(taskSystem, (const signed char *)"taskSystem", TASK_SYSTEM_STACK_SIZE, NULL, 2, NULL);
    xTaskCreate(taskAudio, (const signed char *)"taskAudio", TASK_AUDIO_STACK_SIZE, NULL, 4, NULL);
    vTaskStartScheduler();
    


    while(1);

    return 0;
}

extern "C"
{
void vApplicationStackOverflowHook(xTaskHandle xTask, signed char * pcTaskName)
{
    (void) xTask;
    (void)pcTaskName;

    for(;;);
}
}

#ifdef  USE_FULL_ASSERT
void assert_failed(uint8_t* file, uint32_t line)
{ 
    while (1)
    {
    }
}
#endif
